//------------------------------------------------------------------------------
//
// Dance Service - Performs a very basic "dance"
//
// Written Oct-2007 by Trevor Taylor, Software Technology, Australia
//
//------------------------------------------------------------------------------
// IMPORTANT NOTE!
// This service can be built for three different robots:
// Boe-Bot, LEGO NXT Tribot, or a Simulated robot
//
// Uncomment ONLY ONE of the definitions below for your robot!
// You will get compilation errors if you define more than one.
// 
// The simulation will not work if you build this service with
// either of the real robots because they attempt to connect to
// their respective brick services and the startup will fail.
// You must rebuild it using SIMULATION if you want to run in
// the simulator.
//
//#define BOEBOT
//#define LEGONXT
#define SIMULATION

using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Xml;
using dance = ProMRDS.Robotics.Dance;
using drive = Microsoft.Robotics.Services.Drive.Proxy;
// Added for the Fault class
using W3C.Soap;

// Set up a reference for the relevant brick service
// Only applicable to real robots, not the simulation
#if BOEBOT
using stamp = Microsoft.Robotics.Services.BasicStamp2.Proxy;
#endif
#if LEGONXT
using legonxt = Microsoft.Robotics.Services.Sample.Lego.Nxt.Brick.Proxy;
#endif


namespace ProMRDS.Robotics.Dance
{
    
    /// <summary>
    /// Implementation class for Dance
    /// </summary>
    [DisplayName("Dance")]
    [Description("The Dance Service")]
    [Contract(Contract.Identifier)]
    public class DanceService : DsspServiceBase
    {
        #region constants
        // Constants for motor speeds and timing
        const int repeatCount = 2;         // Number of times to repeat dance

        const int settlingTime = 500;      // Time to wait after each move

        // Values for "exact" movements using DriveDistance and RotateDegrees
        const bool waitForMoves = false;   // Wait for response messages
        const float driveDistance = 0.30f; // Drive 30cm
        const float rotateAngle = 90.0f;   // Turn 90 degrees to the left

        // Constants for the motions using various methods
#if SIMULATION
        const string _platform = "Simulation";
        const bool controlledMoves = false;// Use controlled (not timed) moves
        const float driveSpeed = 0.5f;     // Speed driving forward
        const float rotateSpeed = 0.25f;   // Speed during rotation
        const int driveTime = 1200;        // Time to drive forward (millisec)
        const int rotateTime = 920;        // Time to rotate
#endif
#if LEGONXT
        const string _platform = "LEGO NXT";
        // Values for LEGO NXT using controlled moves
        //const bool controlledMoves = true; // Use controlled (not timed) moves
        //const float driveSpeed = 0.5f;     // Speed driving forward
        //const float rotateSpeed = 0.5f;    // Speed during rotation
        //const int driveTime = 1500;        // Time to drive forward (millisec)
        //const int rotateTime = 1500;       // Time to rotate

        // Values for LEGO NXT using timed moves
        const bool controlledMoves = false;// Use controlled (not timed) moves
        const float driveSpeed = 0.5f;     // Speed driving forward
        const float rotateSpeed = 0.25f;   // Speed during rotation
        const int driveTime = 1200;        // Time to drive forward (millisec)
        const int rotateTime = 920;        // Time to rotate
#endif
#if BOEBOT
        const string _platform = "Boe-Bot";
        // Values for Boe-Bot using firmware
        //const bool controlledMoves = true; // Use controlled (not timed) moves
        //const float driveSpeed = 0.5f;     // Speed driving forward
        //const float rotateSpeed = 0.25f;   // Speed during rotation
        //const int driveTime = 1500;        // Time to drive forward (millisec)
        //const int rotateTime = 1500;       // Time to rotate

        // Values for Boe-Bot using timed moves
        const bool controlledMoves = false;// Use controlled (not timed) moves
        const float driveSpeed = 0.5f;     // Speed driving forward
        const float rotateSpeed = 0.25f;   // Speed during rotation
        const int driveTime = 1800;        // Time to drive forward (millisec)
        const int rotateTime = 950;        // Time to rotate
#endif

        #endregion

        /// <summary>
        /// _state
        /// </summary>
        private DanceState _state = new DanceState();
        /// <summary>
        /// _main Port
        /// </summary>
        [ServicePort("/dance", AllowMultipleInstances=false)]
        private DanceOperations _mainPort = new DanceOperations();

        #region partners

        // Partner with the generic Differential Drive service
        [Partner("Drive", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        drive.DriveOperations _drivePort = new drive.DriveOperations();

        // Set up a partnership with the relevant robot "brick"
        // NOTE: This is not really necessary. If the brick was not used
        // directly then this service would be able to work with any robot
        // that implements the generic differential drive. However, it is
        // intended as an example of using a brick as well.
#if BOEBOT
        // Add a BASICStamp2 partner to allow direct access to its methods
        [Partner("Stamp", Contract = stamp.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        stamp.BasicStamp2Operations _stampPort = new stamp.BasicStamp2Operations();
#endif
#if LEGONXT
        // Add a LEGO NXT V2 partner to allow direct access to its methods
        [Partner("LegoNxtV2", Contract = legonxt.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        legonxt.NxtBrickOperations _legonxtPort = new legonxt.NxtBrickOperations();
#endif

        #endregion

        /// <summary>
        /// Default Service Constructor
        /// </summary>
        public DanceService(DsspServiceCreationPort creationPort) : 
                base(creationPort)
        {
        }
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
			base.Start();
			// Add service specific initialization here.

            // Announce the service AND tell user which platform it supports
            Console.WriteLine("Dance - Built for " + _platform);

            SpawnIterator(Behavior);
        }
        /// <summary>
        /// Get Handler
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public virtual IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
            yield break;
        }

        #region behavior

        // Iterator to execute the Behavior
        // It is important to use an Iterator so that it can relinquish control
        // when there is nothing to do, i.e. yield return
        IEnumerator<ITask> Behavior()
        {
            // Wait for the robot to initialize, otherwise it will
            // miss the initial command
            for (int i = 10; i > 0; i--)
            {
                LogInfo(LogGroups.Console, i.ToString());
                yield return Arbiter.Receive(
                    false,
                    TimeoutPort(1000),
                    delegate(DateTime timeout) { });
            }

            LogInfo(LogGroups.Console, "Starting now ...");

            // Make sure that the drive is enabled first!
            _drivePort.EnableDrive(true);

            // Some bells and whistles ...
#if BOEBOT
            // Make sure the LEDs are set as outputs (not really necessary)
            _stampPort.SetPins(14, (byte)stamp.PinOperations.SET_OUTPUT, 15, (byte)stamp.PinOperations.SET_OUTPUT);
            // Turn both LEDs off (also unnecessary)
            _stampPort.SetPins(14, (byte)stamp.PinOperations.OUTPUT_LOW, 15, (byte)stamp.PinOperations.OUTPUT_LOW);
#endif
#if LEGONXT
            // No initialization required
#endif

            for (int times = 0; times < repeatCount; times++)
            {
                // Drive along the four sides of a square
                for (int side = 0; side < 4; side++)
                {
                    LogInfo(LogGroups.Console, "Side " + side);

                    // Keep the user awake!
#if BOEBOT
                    byte op1, op2;

                    // Toggle the lights to make it pretty and beep as well :-)
                    op1 = ((side & 1) != 0) ? (byte)stamp.PinOperations.OUTPUT_HIGH : (byte)stamp.PinOperations.OUTPUT_LOW;
                    op2 = ((side & 2) != 0) ? (byte)stamp.PinOperations.OUTPUT_HIGH : (byte)stamp.PinOperations.OUTPUT_LOW;
                    // Pins 14 and 15 are LEDs
                    _stampPort.SetPins(14, op1, 15, op2);
                    // The speaker is on pin 4, but we don't need to know this because
                    // the firmware does the tone
                    Activate(
                        Arbiter.Choice(
                            _stampPort.PlayTone(3500 + side * 250, 50),
                            delegate(DefaultUpdateResponseType d)
                            {
                            },
                            delegate(Fault f)
                            {
                                Console.WriteLine("Play Tone failed: " + f.Reason);
                            }
                        )
                    );
#endif
#if LEGONXT
                    // Make a beep (no lights to flash on a LEGO brick!)
                    _legonxtPort.PlayTone(3500 + side * 250, 50);
#endif

                    if (controlledMoves && waitForMoves)
                    {
                        bool success = true;

                        // Drive straight ahead
                        yield return Arbiter.Choice(
                            _drivePort.DriveDistance(driveDistance, driveSpeed),
                            delegate(DefaultUpdateResponseType response) { },
                            delegate(Fault f) { success = false; }
                        );

                        // Wait for settling time
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(settlingTime),
                            delegate(DateTime timeout) { });

                        // Now turn left
                        yield return Arbiter.Choice(
                            _drivePort.RotateDegrees(rotateAngle, rotateSpeed),
                            delegate(DefaultUpdateResponseType response) { },
                            delegate(Fault f) { success = false; }
                        );

                        if (!success)
                            LogError("Error occurred while attempting to drive robot");
                    }
                    else
                    {
                        // Start driving and then wait for a while
                        if (controlledMoves)
                            _drivePort.DriveDistance(driveDistance, driveSpeed);
                        else
                            _drivePort.SetDrivePower(driveSpeed, driveSpeed);

                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(driveTime),
                            delegate(DateTime timeout) { });

                        // Stop the motors and wait for robot to settle
                        _drivePort.SetDrivePower(0, 0);
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(settlingTime),
                            delegate(DateTime timeout) { });

                        // Now turn left and wait for a different amount of time
                        if (controlledMoves)
                            _drivePort.RotateDegrees(rotateAngle, rotateSpeed);
                        else
                            _drivePort.SetDrivePower(-rotateSpeed, rotateSpeed);

                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(rotateTime),
                            delegate(DateTime timeout) { });

                        // Stop the motors and wait for robot to settle
                        _drivePort.SetDrivePower(0, 0);
                        yield return Arbiter.Receive(
                            false,
                            TimeoutPort(settlingTime),
                            delegate(DateTime timeout) { });
                    }
                }
            }

            // And finally make sure that the robot is stopped!
            _drivePort.SetDrivePower(0, 0);

            // Final parting beep
#if BOEBOT
            // And finish off with a sound
            _stampPort.SetPins(14, (byte)stamp.PinOperations.OUTPUT_LOW, 15, (byte)stamp.PinOperations.OUTPUT_LOW);
            Activate(
                Arbiter.Choice(
                    _stampPort.PlayTone(3000, 500),
                    delegate(DefaultUpdateResponseType d)
                    {
                    },
                    delegate(Fault f)
                    {
                        Console.WriteLine("Play Tone failed: " + f.Reason);
                    }
                )
            );
#endif
#if LEGONXT
            _legonxtPort.PlayTone(3000, 500);
#endif

            LogInfo(LogGroups.Console, "Finished");

        }

        #endregion

    }
}
